package com.rgbgame.util;

import processing.core.PApplet;
import com.rgbgame.game.Vector3d;
import com.rgbgame.game.Particle;

/**
 * Provides some utility functions.
 * Might look into wrapping the Processing utility functions here in the future for extensibility.
 * @author Will
 * @date Mar 4, 2008 - 2:09:23 PM
 */
public class Util {

    // Utility function returns array of vectors (positions) from array of
    // particles
    public static Vector3d[] particlesToVectors(Particle[] parts) {
        // debugCounter++;
        Vector3d result[] = new Vector3d[parts.length];
        for (int i = 0; i < parts.length; i++) {
            result[i] = parts[i].position;
        }
        return result;
    }


    // OPT: right now, this is pretty expensive...can we do better?
    // Also, need to change 'factor' so that it is less sensitive to small
    // rotations and more sensitive to large ones - might be fixed now
    Vector3d pBuff = new Vector3d(0, 0, 0); // reserved for pointTransform
    Vector3d pBuff2 = new Vector3d(0, 0, 0); // reserved for pointTransform

    public Vector3d pointTransform(Particle p0, float xIn, float yIn, float smoothedAngularVelocity, float smoothedMouseForce) {
        // return new com.rgbgame.game.Vector3d(xIn,yIn,0);
//        com.rgbgame.game.Particle p0 = myPlayer.getParticle(0);
        // com.rgbgame.game.Vector3d myDisp = p0.position.minus(new
        // com.rgbgame.game.Vector3d(xIn,yIn,0)).times(-1);
        Vector3d myDisp = pBuff;
        myDisp.set(p0.position);
        pBuff2.set(xIn, yIn, 0);
        myDisp.subtract(pBuff2);
        myDisp.multiply(-1.0f);
//        float oldSAV = smoothedAngularVelocity;
        smoothedAngularVelocity = -smoothedMouseForce * .4f + smoothedAngularVelocity * .6f;
        float distance = myDisp.norm();
        float myAlpha = (Math.abs(smoothedAngularVelocity) / (200 + Math
                .abs(smoothedAngularVelocity)));
        float factor = 1.0f
                * (1.0f - myAlpha)
                + ((5 * distance) / ((5 * distance) + 2
                * Math.abs(.2f * smoothedAngularVelocity) + 1))
                * (myAlpha);
        myDisp.multiply(factor * .3f + 1 * .7f);
        float sign;
        if (smoothedAngularVelocity > 0) {
            sign = 1.0f;
        } else {
            sign = -1.0f;
        }
        float angle = -2.0f * sign * (1.0f - factor) / (distance / 300.0f + 1);
        float newX = (float) (myDisp.x * Math.cos(angle) + myDisp.y
                * Math.sin(angle));
        float newY = (float) (-myDisp.x * Math.sin(angle) + myDisp.y
                * Math.cos(angle));
        myDisp.x = newX;
        myDisp.y = newY;
//        smoothedAngularVelocity = oldSAV;
        return p0.position.plus(myDisp);
    }


    public static boolean lineCross(Vector3d wallA, Vector3d wallB,
                                    Vector3d lineA, Vector3d lineB) {
        float x1 = lineA.x;
        float y1 = lineA.y;
        float x2 = lineB.x;
        float y2 = lineB.y;
        float x3 = wallA.x;
        float y3 = wallA.y;
        float x4 = wallB.x;
        float y4 = wallB.y;

        float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
        float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
        float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
        if (PApplet.abs(denom) < .0001f) {
            return false; // skip town if things get tough
        }
        ua /= denom;
        ub /= denom;

        if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) {
            return true;
        }

        return false;
    }

    public static boolean colliding(Particle particle, Vector3d wallA,
                                    Vector3d wallB) {
        // formula from
        // http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
        // This is a line-cross test, which might not be the best one to use.
        // com.rgbgame.game.Vector3d distToWall =
        // particle.position.displacementToLine(wallA,wallB);
        // com.rgbgame.game.Vector3d distToWallLast =
        // particle.positionLast.displacementToLine(wallA,wallB);
        // com.rgbgame.game.Vector3d dirToWall = distToWallLast.normalized();

        float x1 = particle.position.x;
        float y1 = particle.position.y;
        float x2 = particle.positionLast.x;
        float y2 = particle.positionLast.y;
        float x3 = wallA.x;
        float y3 = wallA.y;
        float x4 = wallB.x;
        float y4 = wallB.y;

        float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
        float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
        float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
        if (PApplet.abs(denom) < .0001f) {
            return false;
        }
        ua /= denom;
        ub /= denom;

        if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1)) {

            return true;
        }

        return false;
    }

    public static boolean collideTest(Particle particle, Vector3d wallA,
                                      Vector3d wallB) {
        // debugCounter++;
        // THIS IS NOT THE CORRECT CODE, UPDATE BASED ON SOLIDWALL2D!!!
        if (wallA.distanceTo(wallB) < .001f) {
            return false;
        } // if wallA and wallB are the same point, the collide test goes
        // nutso - should never happen if we design well, but if it does,
        // bail out!
        float speed = PApplet.abs(particle.position.x - particle.positionLast.x)
                + PApplet.abs(particle.position.y - particle.positionLast.y);
        float factor = 2 * speed + 3.0f * particle.radius;// This may need to
        // be tweaked to
        // make sure we
        // don't stupidly
        // miss some
        // collisions...
        if ((particle.positionLast.x < (PApplet.min(wallA.x, wallB.x) - factor))
                || (particle.positionLast.x > (PApplet.max(wallA.x, wallB.x) + factor))) {
            return false;
        }
        if ((particle.positionLast.y < (PApplet.min(wallA.y, wallB.y) - factor))
                || (particle.positionLast.y > (PApplet.max(wallA.y, wallB.y) + factor))) {
            return false;
        }
        Vector3d myDisp = new Vector3d(particle.positionLast
                .displacementToLine(wallA, wallB).normalized().times(
                particle.radius));
        float origx = particle.position.x;
        float origy = particle.position.y;
        particle.position.add(myDisp);
        if (!colliding(particle, wallA, wallB)) {
            particle.position.set(origx, origy, 0);
            // float distToWall = particle.position.distanceToLine(wallA,wallB);
            // if (distToWall < particle.radius){return true;}
            return false;
        }
        particle.position.set(origx, origy, 0);
        return true;
    }


    /**
     * Rotates a 2d vector a certain number of degrees.
     * @param myVec 3d vector (only x and y are used)
     * @param angle degrees in radians.
     */
    public static void rotate2d(Vector3d myVec, float angle) {
        float newX = (float) (myVec.x * Math.cos(angle) + myVec.y
                * Math.sin(angle));
        float newY = (float) (-myVec.x * Math.sin(angle) + myVec.y
                * Math.cos(angle));
        myVec.x = newX;
        myVec.y = newY;
    }

}
